﻿#include "uim.h"
#include "LoadSimCanFunc.h"

UIM::UIM(const QString name,int usb,int com)
{
    Init();
    ret = LoadSimCanFunc(name);
    usbPort = usb;
    comPort = com;
}

UIM::~UIM()
{
    FreeSimCanFunc();
}

void UIM::Init(void)
{
    usbPort = 1;
    comPort = 1;

    canForBackNodeId = 0;
    canLeftNodeId = 0;
    canRightNodeId = 0;
    canPushNodeId = 0;

    isCanLeftMove = false;
    isCanRightMove = false;
    isCanForBackMove = 0;
    isCanPushMove = 0;

    isCanForBackS1 = false;
    isCanForBackS2 = false;
    isCanForBackMoveType = false;

    isCanPushS1 = false;
    isCanPushS2 = false;
    isCanPushMoveType = false;

    isCanForBackEnable = false;
    isCanLeftEnable = false;
    isCanRightEnable = false;
    isCanPushEnable = false;

    memset(&canLeftPara, 0, sizeof(UIM_PARA));
    memset(&canRightPara, 0, sizeof(UIM_PARA));
    memset(&canForBackPara, 0, sizeof(UIM_PARA));
    memset(&canPushPara, 0, sizeof(UIM_PARA));

    setCanForBackRevStep(0);
    setCanPushRevStep(0);

    canLeftOff();
    canRightOff();
    canForBackOff();
    canPushOff();

    canGatewayCount = 0;
    canCurGatewayIndex = -1;
    isCanGateway = false;
}

void UIM::canSearch(void)
{
    if(canGatewayCount > 0 && canCurGatewayIndex > 0){
        return;
    }
    unsigned int uimType=0;
    DEV_INFO_OBJ canDevInfoObj[5];
    int  dwGateWayLen = -1;
    if(usbPort > 0){
        // retInfo("SearchGateway UIDEV_USBCAN");
        uimType |= UIDEV_USBCAN;
    }
    if(comPort > 0){
        // retInfo(QString("SearchGateway UIDEV_RS232CAN "));
        uimType |= UIDEV_RS232CAN;
    }
    dwGateWayLen = SearchGateway(uimType, NULL, canDevInfoObj, 5);
    if (-1 == dwGateWayLen)
    {
        retInfo("Query gateway failed!");
        return;
    }
    canGatewayCount = dwGateWayLen;
    retInfo(QString("Query gateway %0").arg(canGatewayCount));
    for (int  i = 0; i < canGatewayCount; i++)
    {
        canGatewayNameList.append(QString("%0->%1")
                                  .arg(canDevInfoObj[i].pszDevName)
                                  .arg(canDevInfoObj[i].dwDevIndex));
        canGatewayIndex[i]=canDevInfoObj[i].dwDevIndex;
        retInfo(QString("gateway %0->%1:%2")
                .arg(i)
                .arg(canDevInfoObj[i].pszDevName)
                .arg(canDevInfoObj[i].dwDevIndex));
    }
    canCurGatewayIndex = canGatewayIndex[0];
    isCanGateway = false;
}

bool UIM::isOpen(void){
    return isCanGateway;
}

//void UIM::setOpen(bool en){
//    isCanGateway = en;
//}

int UIM::getIndex(void){
    return canCurGatewayIndex;
}

QStringList UIM::getList(void){
    return canGatewayNameList;
}

int UIM::getLeftId(void){
    return canLeftNodeId;
}
int UIM::getLeftPos(void){
    return canGetPos(getLeftId());
}
int UIM::getLeftQec(void){
    return canGetQec(getLeftId());
}
bool UIM::isLeftId(int id){
    if(id == 0){
        return (getLeftId()>0);
    }
    return id == getLeftId();
}
bool UIM::isLeft(void){
    return isCanLeftEnable;
}
bool UIM::getCanLeftMove(void){
    return isCanLeftMove;
}
void UIM::setCanLeftMove(bool mv){
    isCanLeftMove = mv;
}

int UIM::getEncodeLeft(bool Qec){
    if(getLeftId()>0){
        if(Qec){
            return canGetQec(getLeftId());
        }else{
            return canGetPos(getLeftId());
        }
    }
    return 0;
}

int UIM::getEncodeRight(bool Qec){
    if(getRightId()>0){
        if(Qec){
            return canGetQec(getRightId());
        }else{
            return canGetPos(getRightId());
        }
    }
    return 0;
}

int UIM::getRightId(void){
    return canRightNodeId;
}
int UIM::getRightPos(void){
    return canGetPos(getRightId());
}
int UIM::getRightQec(void){
    return canGetQec(getRightId());
}
bool UIM::isRightId(int id){
    if(id == 0){
        return (getRightId()>0);
    }
    return id == getRightId();
}
bool UIM::isRight(void){
    return isCanRightEnable;
}
bool UIM::getCanRightMove(void){
    return isCanRightMove;
}
void UIM::setCanRightMove(bool mv){
    isCanRightMove = mv;
}

int UIM::getForBackId(void){
    return canForBackNodeId;
}
int UIM::getForBackPos(void){
    return canGetPos(canForBackNodeId);
}
int UIM::getForBackStep(void){
    if(isCanForBackMoveType){
        return canForBackPos;
    }else{
        return canForBackStep;
    }
}
bool UIM::isForBackId(int id){
    if(id == 0){
        return (canForBackNodeId>0);
    }
    return id == canForBackNodeId;
}
bool UIM::isForBack(void){
    return isCanForBackEnable;
}
bool UIM::getCanForBack1(void){
    return isCanForBackS1;
}
void UIM::setCanForBack1(bool in){
    isCanForBackS1 = in;
}
bool UIM::getCanForBack2(void){
    return isCanForBackS2;
}
void UIM::setCanForBack2(bool in){
    isCanForBackS2 = in;
}
int UIM::getCanForBackMove(void){
    return isCanForBackMove;
}
void UIM::setCanForBackMove(int mv){
    isCanForBackMove = mv;
}
int UIM::getCanForBackRevStep(void){
    return canForBackRevStep;
}
void UIM::setCanForBackRevStep(int step){
    canForBackRevStep = step;
}

//压片
int UIM::getPushId(void){
    return canPushNodeId;
}

bool UIM::isPushId(int id){
    if(id == 0){
        return (canPushNodeId>0);
    }
    return id == canPushNodeId;
}

bool UIM::isPush(void){
    return isCanPushEnable;
}

int UIM::getPushPos(void){
    return canGetPos(canPushNodeId);
}

int UIM::getPushStep(void){
    if(isCanPushMoveType){
        return canPushPos;
    }else{
        return canPushStep;
    }
}

bool UIM::getCanPush1(void){
    return isCanPushS1;
}
void UIM::setCanPush1(bool in){
    isCanPushS1 = in;
}
bool UIM::getCanPush2(void){
    return isCanPushS2;
}
void UIM::setCanPush2(bool in){
    isCanPushS2 = in;
}

int UIM::getCanPushRevStep(void){
    return canPushRevStep;
}
void UIM::setCanPushRevStep(int step){
    canPushRevStep = step;
}

int UIM::getCanPushMove(void){
    return isCanPushMove;
}
void UIM::setCanPushMove(int mv){
    isCanPushMove = mv;
}

int UIM::compare(const void *first, const void *second)
{
    return *(unsigned int  *)first - *(unsigned int  *)second;
}

bool UIM::canOpenGateWay(void)
{
    //搜索
    canSearch();
    //关闭
    canCloseGateWay();

    MDL_INFO_OBJ canMDLInfoObj[120];
    unsigned int canBtr;
    unsigned int canNodeId[120];
    memset(canNodeId, 0, sizeof(unsigned int )*120);
    memset(canMDLInfoObj, 0, sizeof(MDL_INFO_OBJ)*120);

    canNodeLen = OpenGateway(canCurGatewayIndex, canNodeId, 120, &canBtr);
    if (-1 == canNodeLen)
    {
        retInfo(QString("Open gateway [%0]failed!").arg(canCurGatewayIndex));
        return false;
    }
    QString msg="";
    qsort(canNodeId, canNodeLen, sizeof(unsigned int), compare);
    for(int i  = 0; i < canNodeLen;++i)
    {
        GetMDL(canCurGatewayIndex, canNodeId[i], &(canMDLInfoObj[i]));
        msg +=QString("[%0->%1:%2]").arg(i).arg(canNodeId[i]).arg(canMDLInfoObj[i].szModelName);
    }
    retInfo(QString("open nodeLen=%0->%1").arg(canNodeLen).arg(msg));
    for(int i  = 0; i < canNodeLen;++i)
    {
        //胶片-八个电机赋值
        if(NODEIDFORBACK == (int)canNodeId[i]){
            canForBackNodeId = canNodeId[i];
        }
        if(NODEIDPUSH == (int)canNodeId[i]){
            canPushNodeId = canNodeId[i];
        }
        if(NODEIDLEFT == (int)canNodeId[i]){
            canLeftNodeId = canNodeId[i];
        }
        if(NODEIDRIGHT == (int)canNodeId[i]){
            canRightNodeId = canNodeId[i];
        }
    }
    isCanGateway = true;
    return true;
}

void UIM::canCloseGateWay(void)
{
    if(isCanGateway){
        if (-1 == CloseGateway(canCurGatewayIndex))
        {
            retInfo(QString("Close gateway [%0] failed!").arg(canCurGatewayIndex));
        }
        Init();
    }
}

//使能
bool UIM::canEna(bool &en, int id, const QString msg, UIM_PARA *para){
    if(en){
        return true;
    }
    if(id <= 0){
        return false;
    }

    BASIC_ACK_OBJ objBasicAck;
    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
    if (-1 == UimENA(canCurGatewayIndex, id, 1, &objBasicAck))
    {
        retInfo(QString("can %0 Ena fail!").arg(msg));
        return false;
    }
    en = true;
    // canSetSPD(id,0);
    // canSetSTP(id,0);
    if(para != nullptr){
        para->bENA = objBasicAck.bENA;          //电机使能状态
        para->bDIR = objBasicAck.bDIR;          //电机方向
        para->bACR = objBasicAck.bACR;          //电流减半
        para->uiMCS = objBasicAck.uiMCS;        //电机细分
        para->uiCUR = objBasicAck.uiCUR;        //电流
        para->uiSPD = objBasicAck.uiSPD;        //当前速度
        para->uiSTP = objBasicAck.uiSTP;        //当前步长
        retInfo(QString("%0 canEna para: %1,%2,%3,%4,%5,%6,%7").arg(msg)
                    .arg(para->bENA).arg(para->bDIR).arg(para->bACR).arg(para->uiMCS).arg(para->uiCUR).arg(para->uiSPD).arg(para->uiSTP));
    }
    return true;
}

//关闭
bool UIM::canOff(bool &en, int id, const QString msg, UIM_PARA *para){
    if (!en){
        return true;
    }
    if(id <= 0){
        return false;
    }
    BASIC_ACK_OBJ objBasicAck;
    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
    if (-1 == UimOFF(canCurGatewayIndex, id, 1, &objBasicAck))
    {
        retInfo(QString("can %0 Off fail!").arg(msg));
        return false;
    }
    en = false;
    if(para != nullptr){
        para->bENA = objBasicAck.bENA;          //电机使能状态
        para->bDIR = objBasicAck.bDIR;          //电机方向
        para->bACR = objBasicAck.bACR;          //电流减半
        para->uiMCS = objBasicAck.uiMCS;        //电机细分
        para->uiCUR = objBasicAck.uiCUR;        //电流
        para->uiSPD = objBasicAck.uiSPD;        //当前速度
        para->uiSTP = objBasicAck.uiSTP;        //当前步长
        retInfo(QString("%0 canOff para: %1,%2,%3,%4,%5,%6,%7").arg(msg)
                    .arg(para->bENA).arg(para->bDIR).arg(para->bACR).arg(para->uiMCS).arg(para->uiCUR).arg(para->uiSPD).arg(para->uiSTP));
    }
    return true;
}

UIM_PARA *UIM::getLeftPara(void){
    return &canLeftPara;
}
UIM_PARA *UIM::getRightPara(void){
    return &canRightPara;
}
UIM_PARA *UIM::getForBackPara(void){
    return &canForBackPara;
}

//左电机
void UIM::canLeftEna(bool en){
    if(en){
        isCanLeftEnable = false;
    }
    canEna(isCanLeftEnable,getLeftId(),"left ",&canLeftPara);
}

void UIM::canLeftOff(bool en){
    if(en){
        isCanLeftEnable = true;
    }
    canOff(isCanLeftEnable,getLeftId(),"left ",&canLeftPara);
}

//右电机
void UIM::canRightEna(bool en){
    if(en){
        isCanRightEnable = false;
    }
    canEna(isCanRightEnable,getRightId(),"right ",&canRightPara);
}

void UIM::canRightOff(bool en){
    if(en){
        isCanRightEnable = true;
    }
    canOff(isCanRightEnable,getRightId(),"right ",&canRightPara);
}

//前后
void UIM::canForBackEna(bool en){
    if(en){
        isCanForBackEnable = false;
    }
    canEna(isCanForBackEnable,canForBackNodeId,"forback ",&canForBackPara);
}

void UIM::canForBackOff(bool en){
    if(en){
        isCanForBackEnable = true;
    }
    setCanForBackMove(0);
    canOff(isCanForBackEnable,canForBackNodeId,"forback ",&canForBackPara);
}

void UIM::canPushEna(bool en){
    if(en){
        isCanPushEnable = false;
    }
    canEna(isCanPushEnable,canPushNodeId,"push ",&canPushPara);
}
void UIM::canPushOff(bool en){
    if(en){
        isCanPushEnable = true;
    }
    setCanPushMove(0);
    canOff(isCanPushEnable,canPushNodeId,"push ",&canPushPara);

}

void UIM::canSetSPD(int index,int spd)
{
    if (-1 == SetSPD(canCurGatewayIndex, index, spd, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Set SPD=%1 fail!").arg(index).arg(spd));
        return;
    }
}

//相对位置
void UIM::canSetSTP(int index,int stp)
{
    if (-1 == SetSTP(canCurGatewayIndex, index, stp, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] STP=%1 fail!").arg(index).arg(stp));
        return;
    }
    if(index==canForBackNodeId){
        canForBackStep = stp;
        setCanForBackMove(getIntToMark(stp));
        isCanForBackMoveType = false;
        canForBackPos = 0;
        return;
    }
    if(index==canPushNodeId){
        canPushStep = stp;
        setCanPushMove(getIntToMark(stp));
        isCanPushMoveType = false;
        canPushPos = 0;
        return;
    }
}

void UIM::canStop(int index){
    canSetSTP(index,0);
}

void UIM::canSetOrg(int index)
{
    if (-1 == SetORG(canCurGatewayIndex, index, 0, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] ORG fail!").arg(index));
        return;
    }
    canGetPos(index);
}

int UIM::canGetPos(int index)
{
    int value=-1;
    if(index<=0){
        return value;
    }
    if (-1 == GetPOS(canCurGatewayIndex, index, &value))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Get POS fail!").arg(index));
        return value;
    }
    return value;
}

//绝对位置
void UIM::canSetPOS(int index,int pos)
{
    if (-1 == SetPOS(canCurGatewayIndex, index, pos, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] POS=%1 fail!").arg(index).arg(pos));
        return;
    }
    if(index==canForBackNodeId){
        canForBackPos = pos;
        setCanForBackMove(getIntToMark(pos));
        isCanForBackMoveType = true;
        canForBackStep = 0;
        return;
    }
    if(index==canPushNodeId){
        canPushPos = pos;
        setCanPushMove(getIntToMark(pos));
        isCanPushMoveType = true;
        canPushStep = 0;
        return;
    }
}

void UIM::canSetQec(int index)
{
    int value=0;
    if (-1 == SetQEC(canCurGatewayIndex, index, value, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Set QEC fail!").arg(index));
        return;
    }
    retInfo(QString::fromLocal8Bit("can[%0] Set QEC success!").arg(index));
}

int UIM::canGetQec(int index)
{
    int value=0;
    if (-1 == GetQEC(canCurGatewayIndex, index, &value))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Get QEC fail!").arg(index));
        return value;
    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get QEC %1 success!").arg(index).arg(value));
    return value;
}

// bool UIM::isCanSet(const int node){
//     if(node == getForBackId()){
//         return (getForBackPos() != conf->curSetup()->UIMCanStepForBackLocate);
//     }
//     return false;
// }

//int UIM::canGetMcs(int index)
//{
//    int value=0;
//    BASIC_ACK_OBJ objBasicAck;
//    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
//    if (-1 == GetMCS(canCurGatewayIndex, index, &objBasicAck))
//    {
//        retInfo(QString::fromLocal8Bit("can[%0] Get MCS fail!").arg(index));
//        return value;
//    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get MCS %1 success!").arg(index).arg(value));
//    return value;
//}
//int UIM::canGetCur(int index)
//{
//    int value=0;
//    BASIC_ACK_OBJ objBasicAck;
//    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
//    if (-1 == GetCUR(canCurGatewayIndex, index, &objBasicAck))
//    {
//        retInfo(QString::fromLocal8Bit("can[%0] Get CUR fail!").arg(index));
//        return value;
//    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get CUR %1 success!").arg(index).arg(value));
//    return value;
//}


//int UIM::canSetMcs(int index, int mcs)
//{
//    int value=0;
//    return value;
//}
//int UIM::canSetCur(int index, int cur)
//{
//    int value=0;
//    return value;
//}
